﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

// Projekty zaimportowane ze strony przedmiotu / studia.elka.pw.edu.pl:
using Neural;
using Computing;
using Probability;
using Microsoft.Xna.Framework;

namespace QLearningGame
{
    class QLearningDecisionMaker : Reinforcement.CDDecisionMaker
    {
        private Point viewportSize;

        /// <summary>
        /// Zmienna przechowująca informację o ostatnim znanym stanie środowiska
        /// </summary>
        private double[] currentState;
        private double lastAction = 0;

        Neural.MLPerceptron2 macierzQ;

        /// <summary>
        /// Konstruktor dla klasy pomocniczej Q-Learning.
        /// Najważniejszym zadaniem jest inicjalizacja sieci neuronowej służącej jako aproksymator macierzy Q
        /// w oryginalnej wersji algorytmu.
        /// </summary>
        public QLearningDecisionMaker()
        {
            // Utwórz perceptron
            // Konwencja: na wejściu znormalizowany stan środowiska, na wyjściu decyzja na zasadzie:
            // * liczba <  0.5 => leć do gory,
            // * liczba >= 0.5 => leć w dół
            macierzQ = new MLPerceptron2();

            // ustawienia perceptronu: liczba neuronów w warstwach 1 (Arcustangent) i 2 (1 liniowy na decyzję)
            macierzQ.Build(Settings.wymiarowoscStanu + 1, CellType.Arcustangent, new int[] { 40,1 });
            
            // Skoro deklarujemy takie średnie / odchylenia, to potrzebna będzie normalizacja danych wejściowych
            // z jednostki "odległość w pikselach" na zakres (-1) do 1...
            Vector averages = new Vector(new double[Settings.wymiarowoscStanu + 1]);
            for (int i = 0; i < Settings.wymiarowoscStanu + 1; ++i ) averages[i] = 0;
            Vector stddevs = new Vector(new double[Settings.wymiarowoscStanu + 1] );
            for (int i = 0; i < Settings.wymiarowoscStanu + 1; ++i) stddevs[i] = 1;
            macierzQ.SetInputDescription(averages, stddevs); // scaling the inputs 

            // Losowe wagi połączeń sieci neuronowej na początek
            macierzQ.InitWeights();
        }



        public void CDStartInState(double[] state)
        {
            // Normalizuj liczby opisuące stan do zakresu od -1 do 1, dla lepszego uczenia sieci neuronowej
            state = NormalizeState(state);

            // uaktualnij wiedzę decisionMakera na temat aktualnego stanu środowiska
            this.currentState = state;
        }

        Random r = new Random();

        /// <summary>
        /// Funkcja zwraca najlepsza decyzję wybraną przy pomocy algorytmu Q-Learning
        /// </summary>
        /// <param name="action"></param>
        public void CDAdviseAction(ref int action)
        {
            //action = r.NextDouble() > 0.5 ? 1 : 0;
            //return;

            // Wybór akcji o większej wartości Q (mniejsze szanse na zderzenie)
            double qValue0 = GetQValue(this.currentState, 0);
            double qValue1 = GetQValue(this.currentState, 1);
            int actionDecided = (qValue0 > qValue1 ? 0 : 1);


            // 10 % decyzji czysto losowych
            //if (r.NextDouble() < 0.2) actionDecided = r.NextDouble() > 0.5 ? 1 : 0;
            
            // 0 albo 1 - decyzja losowa z prawdopodobieństwem 0.35 / 0.65
            lastAction = actionDecided;
            action = actionDecided;
        }

        /// <summary>
        /// Pozwala poinformować decydenta o skutkach ostatnio podjętej decyzji i dzięki temu uaktualnić wagi
        /// sieci neuronowej (macierzy Q) aby w przyszłości potwafiła podjąc lepszą decyzję w 
        /// sytuacji podobnej do poprzedniej
        /// </summary>
        /// <param name="reward"></param>
        /// <param name="next_state"></param>
        public void CDThisHappened(double reward, double[] next_state)
        {
            next_state = NormalizeState(next_state);

            // Find maximum reward for both possible actions:
            double nextQ1 = GetQValue(currentState, 0);
            double nextQ2 = GetQValue(currentState, 1);
            double maxNextQ = Math.Max(nextQ1, nextQ2);

            // Update Q matrix
            double oldQ = GetQValue(this.currentState, lastAction);
            double newQ = oldQ + Settings.learningRate*( reward + Settings.discountFactor*maxNextQ - oldQ );
            SetQValue(currentState, lastAction, newQ);

            // Update current state
            this.currentState = next_state;
        }

        /// <summary>
        /// Gets Q value from the perceptron
        /// </summary>
        /// <param name="p"></param>
        /// <param name="lastAction"></param>
        /// <returns></returns>
        private double GetQValue(double[] p, double lastAction)
        {
            Vector input = new Vector(Settings.wymiarowoscStanu + 1);
            // przepisz wejscie ze stanu
            for (int i = 0; i < Settings.wymiarowoscStanu; ++i) input[i] = p[i];
            // i akcji
            input[Settings.wymiarowoscStanu] = lastAction;

            // aktualne wyjście
            Vector output = new Vector(1);
            this.macierzQ.Approximate(input, ref output);

            return output[0];
        }

        private double NormalizeLastAction(double lastAction)
        {
            return lastAction * 2.0d - 1.0d;
        }

        private void SetQValue(double[] p, double lastAction, double value)
        {
            // oczekiwane wyjście = reward
            Vector output_d = new Vector(1);
            output_d[0] = value;

            // aktualne wyjście
            Vector output = new Vector(1);
            output[0] = GetQValue(p, lastAction);

            Vector dq_doutput = null;
            Vector dq_dweights = null;
            dq_doutput = output - output_d;
            macierzQ.BackPropagateGradient(dq_doutput, ref dq_dweights); // only makes sense after Apprximate 
            macierzQ.AddToWeights(dq_dweights, -1e-2); // theta := theta -1e-2 * dq_dweights 
        }


        /// <summary>
        /// Normalizuje liczby opisuące stan (odległości w pikselach), do zakresu od -1 do 1,
        /// dla lepszego uczenia sieci neuronowej
        /// </summary>
        /// <param name="state"></param>
        /// <returns></returns>
        private double[] NormalizeState(double[] state)
        {
            // ujemne mogą być raczej tylko prędkość helikoptera i wysokość względem przeszkody

            // prędkość helikoptera (pionowa, czyli po Y)
            state[0] /= 7;
            // Odległość od sufitu z przodu
            state[1] /= viewportSize.Y;
            // Odległość od podłogi z przodu
            state[2] /= viewportSize.Y;
            // Odległość od przeszkody (tu już po X)
            state[3] /= viewportSize.X;
            // Odległość przeszkody od sufitu (dziura na górze)
            state[4] /= viewportSize.Y;
            // Odległość przeszkody od podłogi (dziura na dole)
            state[5] /= viewportSize.Y;
            // Wysokość helikoptera względem przeszkody (h3, to co ustaliliśmy dzisiaj)
            state[6] /= viewportSize.Y;

            state[7] /= viewportSize.Y;
            
            // normalizacja średniej do 0
            for (int i = 0; i < Settings.wymiarowoscStanu; ++i)
                state[i] = state[i] * 2 - 1;
                
            return state;
        }

        public void SetViewportSize(Point size)
        {
            viewportSize = size;

        }
    }
}
